/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.team3309.beta.subsystems;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.CANJaguar.ControlMode;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.team3309.beta.OI;
import org.team3309.beta.commands.JoystickArm;

/**
 *
 * @author Vincente
 */
public class Arm extends Subsystem {
    private static Arm instance = null;

    private CANJaguar jag;
    private DoubleSolenoid solenoid;
    private double OFFSET = 0;
    private double P=125, I=0, D=0;

    private static final int CAN_ID             = 6;
    private static final int SOLENOID_FORWARD   = 1;
    private static final int SOLENOID_REVERSE   = 2;

    public static Arm getInstance() {
        if(instance == null) {
            instance = new Arm();

            // Set default command here, like this:
            // instance.setDefaultCommand(new CommandIWantToRun());
            instance.setDefaultCommand(new JoystickArm(2));
        }
        return instance;
    }

    // Initialize your subsystem here
    private Arm(){
        try{
            jag = new CANJaguar(CAN_ID);
            changeControlMode(CANJaguar.ControlMode.kPercentVbus);
            System.out.println(jag.getControlMode());
        } catch(CANTimeoutException ex){
            ex.printStackTrace();
        }
        solenoid = new DoubleSolenoid(SOLENOID_FORWARD, SOLENOID_REVERSE);
    }

    public static final class POSITION{
        public static final int START = -75;
        public static final int LEVEL = 0;
        public static final int SCORE = 90;
    }
    
    public void setPID(double P, double I, double D){
        this.P = P;
        this.I = I;
        this.D = D;
    }
    
    public CANJaguar getJaguar(){
        return this.jag;
    }
    
    public void changeControlMode(ControlMode cm){
        try {
            jag.changeControlMode(cm);
            jag.setPID(P, I, D);
            jag.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
            jag.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
            if(cm.equals(CANJaguar.ControlMode.kPosition)||cm.equals(CANJaguar.ControlMode.kSpeed)){
                jag.configEncoderCodesPerRev((short)5);
            }
            if(cm.equals(CANJaguar.ControlMode.kPosition))
                jag.configMaxOutputVoltage(.25);
                jag.enableControl();
            } catch (CANTimeoutException ex) {
                ex.printStackTrace();
        }
    }
    
    public void setPercentVbus(double val){
        //changeControlMode(CANJaguar.ControlMode.kPercentVbus);
        try {
            jag.setX(val);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
    }
    
    public void openGripper(){
        solenoid.set(DoubleSolenoid.Value.kReverse);
    }
    
    public void closeGripper(){
        solenoid.set(DoubleSolenoid.Value.kForward);
    }
    
}